#include "CAFSA.h"

CAFSA::CAFSA(int af_n, int ite, int row, int col, Mat img, float x_mi, float x_ma, float y_mi, float y_ma, float z_mi, float z_ma, int st, float vi, int tn, float df, float ds, float max_b, float max_c, float max_ba, float min_b, float min_c, float min_ba, float wei_b, float wei_c, float wei_ba):valuer(row, col, max_b, max_c, max_ba, min_b, min_c, min_ba, wei_b, wei_c, wei_ba),filter(row, col){

    img.copyTo(frame);
    /*Mat img_ds = filter.wiener(frame, 60, 50);*/
    //imshow("frame", img_ds);
    //float a = valuer.get_img_value(img_ds);
    /*cout << a<<endl;*/

    CAF af(x_mi, x_ma, y_mi, y_ma, z_mi, z_ma, st, vi, tn, df, ds,1.0, 0);
    af_num = af_n;
    ite_num = ite;
    x_best = x_mi;
    y_best = y_mi;

    af.x = rand()/ double(RAND_MAX) *af.x_max + af.x_min;
    af.y = rand()/ double(RAND_MAX) *af.y_max + af.y_min;
    af.z = rand()/ double(RAND_MAX) *af.z_max + af.z_min;
    this->food_con(&af);
    af_social.push_back(af);
    for (int i=1; i<af_num; ++i){
        for (int j=0; j<i; ++j){
            do{
                af.x = rand()/ double(RAND_MAX) *af.x_max + af.x_min;
                af.y = rand()/ double(RAND_MAX) *af.y_max + af.y_min;
                af.z = rand()/ double(RAND_MAX) *af.z_max + af.z_min;
            }while(dstc(af, af_social.at(j)) < 20);
        }
        this->food_con(&af);
        af_social.push_back(af);
    }
    //cout<< af_social.size() <<endl;
    for(int i=0;i<af_num;++i)
    cout<<"NO:"<<i <<", x:"<< af_social.at(i).x <<", y:"<<af_social.at(i).y <<", z:"<<af_social.at(i).z<<", food:"<< af_social.at(i).food<< endl;
}

float CAFSA::dstc(CAF af1, CAF af2){

    return sqrt(pow(af1.x-af2.x, 2)+pow(af1.y-af2.y, 2)+pow(af1.z-af2.z, 2));
}

void CAFSA::food_con(CAF* af){

    af->food = valuer.get_img_value(filter.wiener(frame, af->x, af->y, af->z)) * pow(af->z/150, 0.1);

}

float CAFSA::food_con(CAF af){

    return valuer.get_img_value(filter.wiener(frame, af.x, af.y, af.z))*pow(af.z/150, 0.1);

}

float CAFSA::food_con(float x, float y, float z){

    return valuer.get_img_value(filter.wiener(frame, x, y, z))* pow(z/150, 0.1);
}

void CAFSA::prey(CAF* af){

    float r_visual = 0.0;
    float r_angle = 0.0;
    float z_angle = 0.0;
    float x_n = 0.0;
    float y_n = 0.0;
    float z_n = 0.0;
    float food_n = 0.0;
    //float r_step = 0.0;
    this->food_con(af);

    for(int i=0; i<af->try_num; ++i){
        do{
            r_angle = rand()/ double(RAND_MAX) *2 *3.1415;
            z_angle = rand()/ double(RAND_MAX) *3.1415;
            r_visual = rand()/ double(RAND_MAX) *af->visual;
            x_n = af->x + (r_visual* cos(z_angle) * cos(r_angle));
            y_n = af->y + (r_visual * cos(z_angle)* sin(r_angle));
            z_n = af->z + (r_visual * sin(z_angle));
        }while(x_n<af->x_min || x_n>af->x_max || y_n<af->y_min || y_n>af->y_max || z_n<af->z_min || z_n>af->z_max);
        food_n = this->food_con(x_n, y_n, z_n);
        if(af->food < food_n){
            //r_step = (1-(af->food/food_n))*af->step;
            af->move(x_n, y_n, z_n, 1);
            this->food_con(af);
            af->stay_num = 0;
            cout <<"prey: food_n:"<<food_n<<", x:"<<x_n<<" ,y_n: "<<y_n<<" ,z_n:"<<z_n<<endl;
            return;
        }
    }
    if(!af->best_flag){
        af->move();
        this->food_con(af);
        cout<<"wander:" <<endl;
    }
    else {
        af->stay_num++;
        cout<< "stay:" <<endl;
    }
}

void CAFSA::swarm(CAF* af){

    int n = 0;
    float food_cen = -1000.0;
    float x_cen = 0;
    float y_cen = 0;
    float z_cen = 0;
    float r_step = 0.0;
    float r_angle = 0.0;
    float z_angle = 0.0;
    this->food_con(af);

    for(int i=0; i<af_num; ++i){
        if(dstc(*af, af_social.at(i)) < af->visual){
            n += 1;
            x_cen += af_social.at(i).x;
            y_cen += af_social.at(i).y;
            z_cen += af_social.at(i).z;
            }
        }

    x_cen /= n;
    y_cen /= n;
    z_cen /= n;
    food_cen = this->food_con(x_cen, y_cen, z_cen);

    if((food_cen/n) > (af->delta_s*af->food) && (x_cen) != af->x){
        if(food_cen/af->food >= 1)
            r_step = (1-(af->food/food_cen))*af->step;
        else r_step = (1-(food_cen/af->food))*af->step;
        r_angle = atan2((y_cen-af->y) , (x_cen-af->x));
        z_angle = atan((y_cen-af->z)/sqrt(pow(x_cen-af->x, 2)+pow(y_cen-af->y, 2)));
        af->move(r_step, r_angle, z_angle);
        this->food_con(af);
        af->stay_num = 0;
        cout <<"swarm: x:"<< x_cen<<", y:"<< y_cen<<" ,z:"<< z_cen <<", food:"<<food_cen<<", n:"<<n<<", step:"<<r_step<<", angel:"<<r_angle<<" ,z_angle:"<<z_angle<<endl;
    }
    else this->prey(af);
}

void CAFSA::follow(CAF* af){

    int n = 0;
    float food_max = -1000.0;
    int pos_max = 0;
    //int x_p = af.x;
    //int y_p = af.y;
    float r_step = 0.0;
    float r_angle = 0.0;
    float z_angle = 0.0;
    this->food_con(af);

    for(int i=0; i<af_num; ++i){
        if(this->dstc(*af, af_social.at(i)) < af->visual){
            n += 1;
            this->food_con(&af_social.at(i));
            if(af_social.at(i).food > food_max){
                food_max = af_social.at(i).food;
                pos_max = i;
            }
        }
    }

    if((food_max/n) > (af->delta * af->food) && (af_social.at(pos_max).x) != af->x){

        r_step = (1-(af->food/food_max)) * af->step;
        r_angle = atan2((af_social.at(pos_max).y-af->y) , (af_social.at(pos_max).x-af->x));
        z_angle = atan((af_social.at(pos_max).z-af->z)/sqrt(pow(af_social.at(pos_max).x-af->x, 2)+pow(af_social.at(pos_max).y-af->y, 2)));
        af->move(r_step, r_angle, z_angle);
        this->food_con(af);
        af->stay_num = 0;
        cout <<"follow: x:"<< af_social.at(pos_max).x<<", y:"<< af_social.at(pos_max).y<<" ,z:"<<af_social.at(pos_max).z<<", food:"<<food_max<<", n:"<<n<<", step:"<<r_step<<", angel:"<<r_angle<<" ,z_angle:"<<z_angle<<endl;
    }
    else this->swarm(af);
}

void CAFSA::evaluate(){

    //int pass_time = 0;
    //vector<float> food_best;
    //int x_best_tmp;
    int pos_best;
    float delta_s_tmp_for_best;
    float visual_tmp_for_best;
    float try_tmp_for_best;
    bool conv_flag = 0;
    vector<CAF> af_best;

    for(int j=0; j<ite_num; ++j){

        if(j==0){
            af_best.push_back(af_social.at(0));
            pos_best = 0;
            for(int i=1; i<af_num; ++i){
                if(af_social.at(i).food > af_best.back().food)
                {
                    pos_best = i;
                    af_best.pop_back();
                    af_best.push_back(af_social.at(i));
                }
                cout<< "best pos="<<pos_best<<endl;
            }
            af_social.at(pos_best).best_flag = 1;
        }

        for(int i=0; i<af_num; ++i){
            cout << "NO:"<<i<<", x:"<<af_social.at(i).x<<", y:"<<af_social.at(i).y<<" ,z:"<<af_social.at(i).z<<", food:"<<af_social.at(i).food<<endl;

            if(af_social.at(i).best_flag){
                cout<<"this is best af, stay:"<<af_social.at(i).stay_num<<endl;
                if(af_social.at(i).stay_num > 5)
                {
                    cout<< "convergenced" <<endl<<endl;
                    conv_flag = 1;
                    //break;
                }
                delta_s_tmp_for_best = af_social.at(i).delta_s;
                af_social.at(i).delta_s = 2*delta_s_tmp_for_best;
                visual_tmp_for_best = af_social.at(i).visual;
                af_social.at(i).visual = 1.5*visual_tmp_for_best;
                try_tmp_for_best = af_social.at(i).try_num;
                af_social.at(i).try_num = 1.5*try_tmp_for_best;
            }
            this->follow(&af_social.at(i));
            cout<<"final food:"<<af_social.at(i).food<<", final x:"<<af_social.at(i).x<<", final y:"<<af_social.at(i).y<<", final z:"<<af_social.at(i).z<<endl<<endl;

            if(af_social.at(i).food >= af_best.back().food || i==pos_best){
                if(i!=pos_best){
                    cout<<"last best food: "<<af_best.back().food<<endl;
                    af_social.at(pos_best).best_flag = 0;
                    af_social.at(pos_best).delta_s = delta_s_tmp_for_best;
                    af_social.at(pos_best).visual = visual_tmp_for_best;
                    af_social.at(pos_best).try_num = try_tmp_for_best;
                    pos_best = i;
                    af_social.at(pos_best).best_flag = 1;
                    cout<<"the best changed: "<< pos_best <<endl<<endl;
                }
                af_best.pop_back();
                af_best.push_back(af_social.at(i));
                cout<< "the best af stays: "<<i<<endl<<endl;
            }

        //af_social.at(i).visual -= 4;
        //af_social.at(i).step -= 4;
        }
        af_best.push_back(af_social.at(pos_best));
        cout<<"ite end"<<j<<endl<<endl;
        if ( conv_flag || (af_best.size()>6 && af_best.back().food-af_best.at(af_best.size()-2).food< 0.01 && af_best.at(af_best.size()-2).food - af_best.at(af_best.size()-3).food< 0.01))
            break;
        //valuer.sub_wei(0);
        //valuer.sub_wei(2);
        //valuer.add_wei(1);
        //valuer.add_wei(2);
    }

    for(unsigned int j=0;j<af_best.size(); ++j)
        cout<< af_best.at(j).food<< ","<< af_best.at(j).x<< ","<<af_best.at(j).y<<" ,z:"<<af_best.at(j).z<<endl;

    for(unsigned int i=0;i<af_social.size(); ++i)
    cout <<"NO:"<<i<<",x:"<< af_social.at(i).x <<",y:"<<af_social.at(i).y<<" ,z:"<<af_social.at(i).z<<",food:"<< af_social.at(i).food << endl;
    x_best = af_best.back().x;
    y_best = af_best.back().y;
    z_best = af_best.back().z;
}

void CAFSA::test(){
    clock_t s, t;
    s = clock();
    cout<< af_social.at(0).x<<","<<af_social.at(0).y<<endl;
    this->follow(&af_social.at(0));
    cout<< af_social.at(0).x<<","<<af_social.at(0).y<<endl;
    t = clock();
    double du = (double)(t-s);
    cout<< ",t:"<<du/CLOCKS_PER_SEC <<endl;
}
